<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang.util;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import static imagingbook.lib.math.Arithmetic.isZero;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import static imagingbook.lib.math.Matrix.add;<a name="line.4"></a>
<span class="sourceLineNo">005</span>import static imagingbook.lib.math.Matrix.idMatrix;<a name="line.5"></a>
<span class="sourceLineNo">006</span>import static imagingbook.lib.math.Matrix.multiply;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import static imagingbook.lib.math.Matrix.normL2;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import static imagingbook.lib.math.Matrix.zeroVector;<a name="line.8"></a>
<span class="sourceLineNo">009</span><a name="line.9"></a>
<span class="sourceLineNo">010</span>import org.apache.commons.math3.geometry.euclidean.threed.Rotation;<a name="line.10"></a>
<span class="sourceLineNo">011</span>import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;<a name="line.11"></a>
<span class="sourceLineNo">012</span>import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;<a name="line.12"></a>
<span class="sourceLineNo">013</span><a name="line.13"></a>
<span class="sourceLineNo">014</span>import imagingbook.lib.math.Matrix;<a name="line.14"></a>
<span class="sourceLineNo">015</span>import imagingbook.lib.settings.PrintPrecision;<a name="line.15"></a>
<span class="sourceLineNo">016</span><a name="line.16"></a>
<span class="sourceLineNo">017</span>public class RotationTests {<a name="line.17"></a>
<span class="sourceLineNo">018</span><a name="line.18"></a>
<span class="sourceLineNo">019</span>        public static void main(String[] args) {<a name="line.19"></a>
<span class="sourceLineNo">020</span>                PrintPrecision.set(6);<a name="line.20"></a>
<span class="sourceLineNo">021</span><a name="line.21"></a>
<span class="sourceLineNo">022</span>                double[] axis = {0, -5, 0}; //{1,1,1};<a name="line.22"></a>
<span class="sourceLineNo">023</span>                double theta = normL2(axis); //0.2;<a name="line.23"></a>
<span class="sourceLineNo">024</span>                <a name="line.24"></a>
<span class="sourceLineNo">025</span>                System.out.println("*** Rodrigues vector -&gt; Rotation matrix:\n");<a name="line.25"></a>
<span class="sourceLineNo">026</span>                <a name="line.26"></a>
<span class="sourceLineNo">027</span>                double[] rv = makeRodriguesVector(axis, theta);<a name="line.27"></a>
<span class="sourceLineNo">028</span>                System.out.println("original rv = " + Matrix.toString(rv));<a name="line.28"></a>
<span class="sourceLineNo">029</span>                System.out.println();<a name="line.29"></a>
<span class="sourceLineNo">030</span>                <a name="line.30"></a>
<span class="sourceLineNo">031</span>                double[][] R1 = toRotationMatrix1(rv);<a name="line.31"></a>
<span class="sourceLineNo">032</span>                System.out.println("R1 = \n" + Matrix.toString(R1));<a name="line.32"></a>
<span class="sourceLineNo">033</span>                System.out.println();<a name="line.33"></a>
<span class="sourceLineNo">034</span>                <a name="line.34"></a>
<span class="sourceLineNo">035</span>                double[][] R2 = toRotationMatrix2(rv);<a name="line.35"></a>
<span class="sourceLineNo">036</span>                System.out.println("R2 = \n" + Matrix.toString(R2));<a name="line.36"></a>
<span class="sourceLineNo">037</span>                System.out.println();<a name="line.37"></a>
<span class="sourceLineNo">038</span>                <a name="line.38"></a>
<span class="sourceLineNo">039</span>                //-- going back:<a name="line.39"></a>
<span class="sourceLineNo">040</span>                System.out.println("*** Rotation matrix -&gt; Rodrigues vector:\n");<a name="line.40"></a>
<span class="sourceLineNo">041</span>                <a name="line.41"></a>
<span class="sourceLineNo">042</span>                double[] rv1 = toRodriguesVector1(R1);<a name="line.42"></a>
<span class="sourceLineNo">043</span>                System.out.println("rv1 = " + Matrix.toString(rv1));<a name="line.43"></a>
<span class="sourceLineNo">044</span>                System.out.println("theta1 = " + normL2(rv1));<a name="line.44"></a>
<span class="sourceLineNo">045</span>                System.out.println();<a name="line.45"></a>
<span class="sourceLineNo">046</span>                <a name="line.46"></a>
<span class="sourceLineNo">047</span>                double[] rv2 = toRodriguesVector2(R1);<a name="line.47"></a>
<span class="sourceLineNo">048</span>                System.out.println("rv2 = " + Matrix.toString(rv2));<a name="line.48"></a>
<span class="sourceLineNo">049</span>                System.out.println("theta2 = " + normL2(rv2));<a name="line.49"></a>
<span class="sourceLineNo">050</span>                System.out.println();<a name="line.50"></a>
<span class="sourceLineNo">051</span>        }<a name="line.51"></a>
<span class="sourceLineNo">052</span>        <a name="line.52"></a>
<span class="sourceLineNo">053</span>        static double[] makeRodriguesVector(double[] axis, double theta) {<a name="line.53"></a>
<span class="sourceLineNo">054</span>                double s = normL2(axis);<a name="line.54"></a>
<span class="sourceLineNo">055</span>                return multiply(theta / s, axis);<a name="line.55"></a>
<span class="sourceLineNo">056</span>        }<a name="line.56"></a>
<span class="sourceLineNo">057</span>        <a name="line.57"></a>
<span class="sourceLineNo">058</span>        // ++++++++++++++   Rodrigues vector --&gt; Rotation matrix  +++++++++++++++++++<a name="line.58"></a>
<span class="sourceLineNo">059</span>        <a name="line.59"></a>
<span class="sourceLineNo">060</span>        /**<a name="line.60"></a>
<span class="sourceLineNo">061</span>         * Hand-made calculation (no library)<a name="line.61"></a>
<span class="sourceLineNo">062</span>         * @param rv Rodrigues rotation vector<a name="line.62"></a>
<span class="sourceLineNo">063</span>         * @return<a name="line.63"></a>
<span class="sourceLineNo">064</span>         */<a name="line.64"></a>
<span class="sourceLineNo">065</span>        static double[][] toRotationMatrix1(double[] rv) {<a name="line.65"></a>
<span class="sourceLineNo">066</span>                double theta = normL2(rv);<a name="line.66"></a>
<span class="sourceLineNo">067</span>                double rx = rv[0] / theta;<a name="line.67"></a>
<span class="sourceLineNo">068</span>                double ry = rv[1] / theta;<a name="line.68"></a>
<span class="sourceLineNo">069</span>                double rz = rv[2] / theta;<a name="line.69"></a>
<span class="sourceLineNo">070</span>                <a name="line.70"></a>
<span class="sourceLineNo">071</span>                System.out.println("rotation angle1 = " + theta);<a name="line.71"></a>
<span class="sourceLineNo">072</span>                System.out.println("rotation axis1 = " + Matrix.toString(new double[] {rx, ry, rz}));<a name="line.72"></a>
<span class="sourceLineNo">073</span>                <a name="line.73"></a>
<span class="sourceLineNo">074</span>                double[][] W = {<a name="line.74"></a>
<span class="sourceLineNo">075</span>                                {0, -rz, ry},<a name="line.75"></a>
<span class="sourceLineNo">076</span>                                {rz, 0, -rx},<a name="line.76"></a>
<span class="sourceLineNo">077</span>                                {-ry, rx, 0}};<a name="line.77"></a>
<span class="sourceLineNo">078</span>                //System.out.println("W = \n" + Matrix.toString(W));<a name="line.78"></a>
<span class="sourceLineNo">079</span>                double[][] I = idMatrix(3);<a name="line.79"></a>
<span class="sourceLineNo">080</span>                double[][] R1 = add(I, multiply(Math.sin(theta), W));<a name="line.80"></a>
<span class="sourceLineNo">081</span>                double[][] R2 = multiply(1 - Math.cos(theta), multiply(W,W));<a name="line.81"></a>
<span class="sourceLineNo">082</span>                double[][] R = add(R1, R2);<a name="line.82"></a>
<span class="sourceLineNo">083</span>                <a name="line.83"></a>
<span class="sourceLineNo">084</span>                return R;<a name="line.84"></a>
<span class="sourceLineNo">085</span>        }<a name="line.85"></a>
<span class="sourceLineNo">086</span>        <a name="line.86"></a>
<span class="sourceLineNo">087</span>        /**<a name="line.87"></a>
<span class="sourceLineNo">088</span>         * For comparison, this version uses Apache CM.<a name="line.88"></a>
<span class="sourceLineNo">089</span>         * @param rv<a name="line.89"></a>
<span class="sourceLineNo">090</span>         * @return<a name="line.90"></a>
<span class="sourceLineNo">091</span>         */<a name="line.91"></a>
<span class="sourceLineNo">092</span>        static double[][] toRotationMatrix2(double[] rv) {<a name="line.92"></a>
<span class="sourceLineNo">093</span>                double angle = normL2(rv);<a name="line.93"></a>
<span class="sourceLineNo">094</span>                Vector3D axis = new Vector3D(rv);<a name="line.94"></a>
<span class="sourceLineNo">095</span>                Rotation rotation = new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR);<a name="line.95"></a>
<span class="sourceLineNo">096</span>                System.out.println("rotation angle2 = " + rotation.getAngle());<a name="line.96"></a>
<span class="sourceLineNo">097</span>                System.out.println("rotation axis2 = " + rotation.getAxis(RotationConvention.VECTOR_OPERATOR));<a name="line.97"></a>
<span class="sourceLineNo">098</span>                return rotation.getMatrix();<a name="line.98"></a>
<span class="sourceLineNo">099</span>        }<a name="line.99"></a>
<span class="sourceLineNo">100</span>        <a name="line.100"></a>
<span class="sourceLineNo">101</span>        // ++++++++++++++   Rotation matrix --&gt; Rodrigues vector +++++++++++++++++++<a name="line.101"></a>
<span class="sourceLineNo">102</span>        <a name="line.102"></a>
<span class="sourceLineNo">103</span>        /**<a name="line.103"></a>
<span class="sourceLineNo">104</span>         * from "`Vector Representation of Rotations"', Carlo Tomasi<a name="line.104"></a>
<span class="sourceLineNo">105</span>         * https://www.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.pdf<a name="line.105"></a>
<span class="sourceLineNo">106</span>         * Matlab code: http://www.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.m<a name="line.106"></a>
<span class="sourceLineNo">107</span>         * @param R<a name="line.107"></a>
<span class="sourceLineNo">108</span>         * @return<a name="line.108"></a>
<span class="sourceLineNo">109</span>         */<a name="line.109"></a>
<span class="sourceLineNo">110</span>        static double[] toRodriguesVector1(double[][] R) {<a name="line.110"></a>
<span class="sourceLineNo">111</span>//              final double eps = EPSILON_DOUBLE;<a name="line.111"></a>
<span class="sourceLineNo">112</span>                <a name="line.112"></a>
<span class="sourceLineNo">113</span>                double[] p = {<a name="line.113"></a>
<span class="sourceLineNo">114</span>                                0.5 * (R[2][1] - R[1][2]), <a name="line.114"></a>
<span class="sourceLineNo">115</span>                                0.5 * (R[0][2] - R[2][0]), <a name="line.115"></a>
<span class="sourceLineNo">116</span>                                0.5 * (R[1][0] - R[0][1])};<a name="line.116"></a>
<span class="sourceLineNo">117</span>                double s = normL2(p);<a name="line.117"></a>
<span class="sourceLineNo">118</span>                double c = 0.5 * (Matrix.trace(R) - 1);<a name="line.118"></a>
<span class="sourceLineNo">119</span>                <a name="line.119"></a>
<span class="sourceLineNo">120</span>                if (isZero(s)) {                      // Rotation angle is either 0 or pi<a name="line.120"></a>
<span class="sourceLineNo">121</span>                        if (isZero(c - 1)) {       // Case 1: c = 1, Rotation angle is 0<a name="line.121"></a>
<span class="sourceLineNo">122</span>                                return zeroVector(3);<a name="line.122"></a>
<span class="sourceLineNo">123</span>                        }<a name="line.123"></a>
<span class="sourceLineNo">124</span>                        else if (isZero(c + 1)) {    // Case 2: c = -1, Rotation angle is pi<a name="line.124"></a>
<span class="sourceLineNo">125</span>                                // find the column of R + I with greatest norm (for better numerical results)<a name="line.125"></a>
<span class="sourceLineNo">126</span>                                double[][] Rp = add(R, idMatrix(3));<a name="line.126"></a>
<span class="sourceLineNo">127</span>                                double[] v = getMaxColumnVector(Rp);<a name="line.127"></a>
<span class="sourceLineNo">128</span>                                double vn = normL2(v);<a name="line.128"></a>
<span class="sourceLineNo">129</span>                                if (isZero(vn)) {   // this shouldn't really happen<a name="line.129"></a>
<span class="sourceLineNo">130</span>                                        throw new RuntimeException("R is an inversion, not a rotation");<a name="line.130"></a>
<span class="sourceLineNo">131</span>                                }<a name="line.131"></a>
<span class="sourceLineNo">132</span>                                double[] u = multiply(1 / vn, v);       // unit vector<a name="line.132"></a>
<span class="sourceLineNo">133</span>                                return multiply(Math.PI, S(u));<a name="line.133"></a>
<span class="sourceLineNo">134</span>                        }<a name="line.134"></a>
<span class="sourceLineNo">135</span>                        else {                  // how can this be?<a name="line.135"></a>
<span class="sourceLineNo">136</span>                                throw new RuntimeException("sin(theta) is zero, bus cos(theta) is neither 1 nor -1!");<a name="line.136"></a>
<span class="sourceLineNo">137</span>                        }<a name="line.137"></a>
<span class="sourceLineNo">138</span>                }<a name="line.138"></a>
<span class="sourceLineNo">139</span>                else  {   // (s != 0) : rotation strictly between 0 and pi<a name="line.139"></a>
<span class="sourceLineNo">140</span>                        double[] u = multiply(1 / s, p);        // unit vector<a name="line.140"></a>
<span class="sourceLineNo">141</span>                        double theta = Math.atan2(s, c);<a name="line.141"></a>
<span class="sourceLineNo">142</span>                        return multiply(theta, u);<a name="line.142"></a>
<span class="sourceLineNo">143</span>                }<a name="line.143"></a>
<span class="sourceLineNo">144</span>        }<a name="line.144"></a>
<span class="sourceLineNo">145</span>        <a name="line.145"></a>
<span class="sourceLineNo">146</span>        /**<a name="line.146"></a>
<span class="sourceLineNo">147</span>         * Changes the sign of a unit vector u so that it is on the proper half of<a name="line.147"></a>
<span class="sourceLineNo">148</span>         * the unit sphere.<a name="line.148"></a>
<span class="sourceLineNo">149</span>         * @param x unit vector<a name="line.149"></a>
<span class="sourceLineNo">150</span>         * @return same or inverted unit vector<a name="line.150"></a>
<span class="sourceLineNo">151</span>         */<a name="line.151"></a>
<span class="sourceLineNo">152</span>        private static double[] S(double[] x) {<a name="line.152"></a>
<span class="sourceLineNo">153</span>//              if ((u[0] == 0 &amp;&amp; u[1] == 0 &amp;&amp; u[2] &lt; 0) || (u[0] == 0 &amp;&amp; u[1] &lt; 0) || u[0] &lt; 0) <a name="line.153"></a>
<span class="sourceLineNo">154</span>                if ((x[0] &lt; 0) ||<a name="line.154"></a>
<span class="sourceLineNo">155</span>                        (isZero(x[0]) &amp;&amp; x[1] &lt; 0) ||<a name="line.155"></a>
<span class="sourceLineNo">156</span>                        (isZero(x[0]) &amp;&amp; isZero(x[1]) &amp;&amp; x[2] &lt; 0))     {<a name="line.156"></a>
<span class="sourceLineNo">157</span>                        return multiply(-1, x);<a name="line.157"></a>
<span class="sourceLineNo">158</span>                }<a name="line.158"></a>
<span class="sourceLineNo">159</span>                else {<a name="line.159"></a>
<span class="sourceLineNo">160</span>                        return x;<a name="line.160"></a>
<span class="sourceLineNo">161</span>                }<a name="line.161"></a>
<span class="sourceLineNo">162</span>        }<a name="line.162"></a>
<span class="sourceLineNo">163</span>        <a name="line.163"></a>
<span class="sourceLineNo">164</span>//      private static boolean isZero(double x) {<a name="line.164"></a>
<span class="sourceLineNo">165</span>//              return Math.abs(x) &lt; EPSILON_DOUBLE;<a name="line.165"></a>
<span class="sourceLineNo">166</span>//      }<a name="line.166"></a>
<span class="sourceLineNo">167</span>        <a name="line.167"></a>
<span class="sourceLineNo">168</span>        private static double[] getMaxColumnVector(double[][] A) {<a name="line.168"></a>
<span class="sourceLineNo">169</span>                final int rows = A.length;<a name="line.169"></a>
<span class="sourceLineNo">170</span>                final int cols = A[0].length;<a name="line.170"></a>
<span class="sourceLineNo">171</span>                int maxCol = 0;<a name="line.171"></a>
<span class="sourceLineNo">172</span>                double maxNorm = Double.NEGATIVE_INFINITY;<a name="line.172"></a>
<span class="sourceLineNo">173</span>                for (int c = 0; c &lt; cols; c++) {<a name="line.173"></a>
<span class="sourceLineNo">174</span>                        double csum = 0;<a name="line.174"></a>
<span class="sourceLineNo">175</span>                        for (int r = 0; r &lt; rows; r++) {<a name="line.175"></a>
<span class="sourceLineNo">176</span>                                csum = csum + A[r][c] * A[r][c];<a name="line.176"></a>
<span class="sourceLineNo">177</span>                        }<a name="line.177"></a>
<span class="sourceLineNo">178</span>                        if (csum &gt; maxNorm) {<a name="line.178"></a>
<span class="sourceLineNo">179</span>                                maxNorm = csum;<a name="line.179"></a>
<span class="sourceLineNo">180</span>                                maxCol = c;<a name="line.180"></a>
<span class="sourceLineNo">181</span>                        }<a name="line.181"></a>
<span class="sourceLineNo">182</span>                }<a name="line.182"></a>
<span class="sourceLineNo">183</span>                return Matrix.getColumn(A, maxCol);<a name="line.183"></a>
<span class="sourceLineNo">184</span>        }<a name="line.184"></a>
<span class="sourceLineNo">185</span>        <a name="line.185"></a>
<span class="sourceLineNo">186</span>        // http://math.stackexchange.com/questions/83874/efficient-and-accurate-numerical-implementation-of-the-inverse-rodrigues-rotatio<a name="line.186"></a>
<span class="sourceLineNo">187</span>        <a name="line.187"></a>
<span class="sourceLineNo">188</span>        <a name="line.188"></a>
<span class="sourceLineNo">189</span>        /**<a name="line.189"></a>
<span class="sourceLineNo">190</span>         * Correct!<a name="line.190"></a>
<span class="sourceLineNo">191</span>         * @param R<a name="line.191"></a>
<span class="sourceLineNo">192</span>         * @return<a name="line.192"></a>
<span class="sourceLineNo">193</span>         */<a name="line.193"></a>
<span class="sourceLineNo">194</span>        static double[] toRodriguesVector2(double[][] R) {<a name="line.194"></a>
<span class="sourceLineNo">195</span>                Rotation rot = new Rotation(R, 0.01);<a name="line.195"></a>
<span class="sourceLineNo">196</span>                double angle = rot.getAngle();<a name="line.196"></a>
<span class="sourceLineNo">197</span>                Vector3D axis = rot.getAxis( RotationConvention.VECTOR_OPERATOR);       <a name="line.197"></a>
<span class="sourceLineNo">198</span>                double[] rv = axis.scalarMultiply(angle/axis.getNorm()).toArray();<a name="line.198"></a>
<span class="sourceLineNo">199</span>                return rv;<a name="line.199"></a>
<span class="sourceLineNo">200</span>        }<a name="line.200"></a>
<span class="sourceLineNo">201</span>        <a name="line.201"></a>
<span class="sourceLineNo">202</span><a name="line.202"></a>
<span class="sourceLineNo">203</span><a name="line.203"></a>
<span class="sourceLineNo">204</span>}<a name="line.204"></a>




























































</pre>
</div>
</body>
</html>
